#ifndef VISIONNODE_H_
#define VISIONNODE_H_

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "std_msgs/String.h"
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include "../utilities/Constants.h"

#include <boost/thread.hpp>

using namespace std;
namespace std {
using boost::mutex;
using boost::condition_variable;
using boost::unique_lock;
using boost::thread;
}

namespace enc = sensor_msgs::image_encodings;
using namespace cv;

/**
 * Node for acquiring object's position in image received from Kinect.
 * The object is recognized according to its color.
 * The color is set by publishing to /VisionNode/SetColor
 */
class VisionNode {

private:

	// ROS management objects
	ros::NodeHandle m_node_handler;
	sensor_msgs::Image m_image;
	ros::Subscriber m_cloud_sub;
	ros::Publisher m_obj_pos_publisher;
	std_msgs::String m_message_to_publish;
	ros::Subscriber m_color_sub;

	// used for synchronized access to data
	std::mutex m_mutex;

	// bounds for color to detect
	CvScalar m_lower_bound_hsv;
	CvScalar m_upper_bound_hsv;

	map<string, string> m_params; // parameters parsed out of params.xml

	int m_min_obj_size; // minimal contour (in pixels) for an object

	/**
	 * Checks if target position is valid.
	 * A valid position in the point cloud is one that its (x,y,z) values
	 * are not NaN. \n
	 * Invalid positions are usually points that are too close to the camera or
	 * are too aside for the camera
	 * @param target position in the point cloud
	 * @return True if position is valid, False if not
	 */
	bool isValidPos(pcl::PointXYZ target);

	/**
	 * Parses CvScalar for the lower and upper bounds, that define
	 * the color threshold to find
	 * @param msg topic message holding bounds of color to find
	 * @param lower_bound_hsv HSV color lower bounds values
	 * @param upper_bound_hsv HSV color upper bounds values
	 */
	void parseMsg(string msg, CvScalar& lower_bound_hsv,CvScalar& upper_bound_hsv);

public:

	/**
	 * Constructor- initializes the vision node: \n
	 * sets default color according to parameters in XML
	 * and subscribes to the topics
	 * @throws error message if had errors parsing XML parameters file
	 */
	VisionNode() throw(const char*);

	/**
	 * Destructor- virtual, to allow inheritance
	 */
	virtual ~VisionNode();

	/**
	 * Callback for /camera/depth_registered/points. \n
	 * Given a point cloud acquired from the topic, finds biggest blob
	 * of the color to track (ignores blobs that are too small). \n
	 * If could detect the object, its position and blob size are updated. \n
	 * If not, position is set to error value. \n
	 * If could detect object but Kinect could not measure its position (NaN
	 * values), then distance value in object position is set as (-1)*abs(previous_position)
	 * @param msg point cloud message
	 */
	void detectObjCB(const sensor_msgs::PointCloud2ConstPtr& msg);

	/**
	 * Callback for /VisionNode/SetColor. \n
	 * Sets color to be found (by \ref detectObjCB()) as the value indicated in the message. \n
	 * Message format is HSV values for lower bound and then HSV values for upper bounds,
	 * separated by DELIMITER (from config.xml). \n
	 * For example, if DELIMITER is ":", lower bound values are (10,20,30) and upper bound
	 * values are (40,50,60), then the message would be: \n
	 * 10:20:30:40:50:60 \n
	 * ATTENTION: hue values are divided by 2 (as indicated by OpenCV documentation). That is,
	 * if parsing hue value of 10, then OpenCV would consider it as 20
	 * @param msg topic message holding bounds of color to find
	 */
	void setColorCB(const std_msgs::String::ConstPtr& msg);
};

#endif /* VISIONNODE_H_ */
